package frc.circuitrunners;

/** Imports */
import edu.wpi.first.wpilibj.can.CANTimeoutException;
import edu.wpi.first.wpilibj.CANJaguar;
import edu.wpi.first.wpilibj.Victor;

/**
 * @author CircuitRunners
 * This package and class contain all implementation of the Drive code
 */
public class Drive extends RobotSubsystem {
    
    /**
     * Jaguars that power the drive train
     */
    private boolean practiceBot = true;
    private CANJaguar FrontRight, FrontLeft, BackRight, BackLeft;
    private Victor FR, FL, BR, BL;
    //private Gyro robotGyro;
    
    /**
     * Run a stage of the autonomous process
     * 
     */
    public void autonomous() {
        switch(stage){
            
        }
    }
    
    /**
     * Initializing the drivetrain
     */
    public void init() {
        Scheduler.feed();
        if(practiceBot){
            FR = new Victor(1);
            FL = new Victor(2);
            BR = new Victor(3);
            BL = new Victor(4);
        } else {
            try {
                FrontRight = new CANJaguar(1); //Initialize front right motor
                FrontLeft = new CANJaguar(2); //Initialzie front left motor
                BackRight = new CANJaguar(3); //Initialize back right motor
                BackLeft = new CANJaguar(4); //Initialize back left motor
                FrontRight.setVoltageRampRate(0.0);
                FrontLeft.setVoltageRampRate(0.0);
                BackRight.setVoltageRampRate(0.0);
                BackLeft.setVoltageRampRate(0.0);
                //robotGyro = new Gyro(2,1); //Initialize Gyro
            } catch (CANTimeoutException GGYOUFAIL) {
                Scheduler.yield();
            }
        }
    }

    /**
     * Run the Teleop code written here for the drivetrain
     */
    public void teleop() {
        Scheduler.feed();
        double x, y, z; //variables to store joystick input
        x = IO.getJoystickX(IO.JOY_DRIVE);
        y = IO.getJoystickY(IO.JOY_DRIVE);
        z = IO.getJoystickZ(IO.JOY_DRIVE);
        if(Math.abs(z) < 0.1){
            z = 0;
        }
        if(Math.abs(x) < 0.1){
            x = 0;
        }
        if(Math.abs(y) < 0.1){
            y = 0;
        }
        driveCartesian(x, y ,z);
    }
    
    /**
     * Limits our input or output to the normalized -1.0 to 1.0
     * @param input the input
     * @return the limited output
     */
    private double limit(double input){
        if(input >= 1.0){ //if greater than 1.0
            return 1.0; //return 1.0
        } else if(input <= -1.0){ //if less than -1.0
            return -1.0; //return -1.0
        } else { //otherwise
            return input; //return the input
        }
    }
    
    public void drivePolar(double r, double theta, double rotation){
        double x, y, z;
        x = r * Math.cos(theta);
        y = r * Math.sin(theta);
        z = rotation;
        driveCartesian(x, y, z);
    }
    
    public void driveCartesian(double x, double y, double z){
        double fr, fl, br, bl;
        z = -z;
        y = -y;
        fr = -(x + y + z);
        fl = x - y - z;
        br = -(x - y + z);
        bl = x + y - z;
        if(practiceBot){
            FR.set(limit(fr));
            FL.set(limit(fl));
            BR.set(limit(br));
            BL.set(limit(bl));
        } else {
            try{
                FrontRight.setX(limit(fr)); //set front right motor to normalized calculated speed
                FrontLeft.setX(limit(fl)); //set front left motor to normalized calculated speed
                BackRight.setX(limit(br)); //set back right motor to normalized calculated speed
                BackLeft.setX(limit(bl)); //set back left motor to normalized calculated speed
            } catch(CANTimeoutException cantoe){
                //ignore
            }   
        }
        DashboardHandler.FR = fr;
        DashboardHandler.FL = fl;
        DashboardHandler.BR = br;
        DashboardHandler.BL = bl;
    }
    
    /**
     * Move the robot to x,y coordinate
     * @param x the x coordinate 
     * @param y the y coordinate
     * @param speed the speed which the robot shall move. between -1.0 and 1.0.
     */
    public void move(double x, double y, double speed){
        
    }
    
    /**
     * Move in a straight line at an angle theta from the horizontal
     * @param theta the angle from the horizontal the robot should move
     * Note, this is not a robot heading!!!! This is for moving diagonally or in any direction
     */
    public void directionalMove(double r, double theta){
        
    }
    
    /**
     * Drive completely stops when auto aiming
     */
    public void autoAim() {
        driveCartesian(0.0,0.0,IO.CameraHandler.rotationSpeed);
    }
    
    /**
     * Stops robot completely when disabled
     */
    public void disable(){
        Scheduler.feed();
        if(practiceBot){
            FR.set(0);
            FL.set(0);
            BR.set(0);
            BL.set(0);
        } else {
            try {  
                FrontRight.setX(0); //stops front right motor
                FrontLeft.setX(0);  //stops front left motor
                BackRight.setX(0); //stops back right motor
                BackLeft.setX(0); //stops back left motor
            } catch (CANTimeoutException ex) {
                Scheduler.yield();
            }
        }
    }

    public void autoBalance() {
    }
}
